using System;
using Microsoft.Xna.Framework;

namespace SolarWinds.Helpers
{
    public class BoundingSphereHelper
    {
        public static bool SphereFit3(Vector3[] points, int iMaxIterations, ref BoundingSphere sphere)
        {
            // compute the average of the data points
            Vector3 kAverage = points[0];
            for (int i0 = 1; i0 < points.Length; i0++)
            {
                kAverage += points[i0];
            }
            float fInvQuantity = 1.0f / points.Length;
            kAverage *= fInvQuantity;

            sphere.Center = kAverage;

            int i1;
            for (i1 = 0; i1 < iMaxIterations; ++i1)
            {
                Vector3 kDiff;

                // update the iterates
                Vector3 kCurrent = sphere.Center;

                // compute average L, dL/da, dL/db, dL/dc
                float fLAverage = 0.0f;
                Vector3 kDerLAverage = Vector3.Zero;
                for (int i0 = 0; i0 < points.Length; i0++)
                {
                    kDiff = points[i0] - sphere.Center;
                    float fLength = kDiff.Length();
                    if (fLength > float.Epsilon)
                    {
                        fLAverage += fLength;
                        float fInvLength = ((float)1.0) / fLength;
                        kDerLAverage -= fInvLength * kDiff;
                    }
                }
                fLAverage *= fInvQuantity;
                kDerLAverage *= fInvQuantity;

                sphere.Center = kAverage + fLAverage * kDerLAverage;
                sphere.Radius = fLAverage;

                kDiff = sphere.Center - kCurrent;
                if (Math.Abs(kDiff.X) <= float.Epsilon &&
                    Math.Abs(kDiff.Y) <= float.Epsilon &&
                    Math.Abs(kDiff.Z) <= float.Epsilon)
                {
                    break;
                }
            }

            return i1 < iMaxIterations;
        }

        public static void CalcSphere(Vector3[] points, ref BoundingSphere sphere)
        {
            // find center
            sphere.Center = Vector3.Zero;

            for (int i = 0; i < points.Length; i++)
            {
                sphere.Center += points[i];
            }

            sphere.Center /= (float)points.Length;

            // find farthest point in set
            sphere.Radius = 0.0f;

            for (int i = 0; i < points.Length; i++)
            {
                float dist = (points[i] - sphere.Center).LengthSquared();

                if (dist > sphere.Radius)

                    sphere.Radius = dist;
            }

            sphere.Radius = (float)Math.Sqrt(sphere.Radius);
        }
    }
}